/*
 * FlightController_Mode.cpp
 *
 *  Created on: Jan 31, 2014
 *      Author: ppks
 */
#include "common.h"
#include "FlightController.h"
#include "FlightController_PID.h"
#include "FlightController_StickCmd.h"
#include "FlightController_Mode.h"
#include "FlightController_Failsafe.h"
#include "GPS_common.h"
#include "avr/pgmspace.h"

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
/* names for dynamic generation of config GUI */
const char BOXnames[] PROGMEM =
    "ARM;"
#if ACC_INSTALLED
    "ANGLE;"
    "HORIZON;"
#endif /* ACC_INSTALLED */
#if BARO_INSTALLED && (!defined(SUPPRESS_BARO_ALTHOLD))
    "BARO;"
#endif /* BARO_INSTALLED && (!defined(SUPPRESS_BARO_ALTHOLD)) */
#ifdef VARIOMETER
    "VARIO;"
#endif /* VARIOMETER */
#if MAG_INSTALLED
    "MAG;"
    "HEADFREE;"
    "HEADADJ;"
#endif /* MAG_INSTALLED */
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
    "CAMSTAB;"
#endif /* #if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT) */
#if defined(CAMTRIG)
    "CAMTRIG;"
#endif /* defined(CAMTRIG) */
#if GPS_INSTALLED
    "GPS HOME;"
    "GPS HOLD;"
#endif /* GPS_INSTALLED */
#if defined(FIXEDWING) || defined(HELICOPTER)
    "PASSTHRU;"
#endif /* defined(FIXEDWING) || defined(HELICOPTER) */
#if defined(BUZZER)
    "BEEPER;"
#endif /* defined(BUZZER) */
#if defined(LED_FLASHER)
    "LEDMAX;"
    "LEDLOW;"
#endif /* defined(LED_FLASHER) */
#if defined(LANDING_LIGHTS_DDR)
    "LLIGHTS;"
#endif /* defined(LANDING_LIGHTS_DDR) */
#ifdef INFLIGHT_ACC_CALIBRATION
    "CALIB;"
#endif /* INFLIGHT_ACC_CALIBRATION */
#ifdef GOVERNOR_P
    "GOVERNOR;"
#endif /* GOVERNOR_P */
#ifdef OSD_SWITCH
    "OSD SW;"
#endif /* OSD_SWITCH */
#if GPS_NAVIGATION_ENABLED
    "MISSION;"
    "LAND;"
#endif /* GPS_NAVIGATION_ENABLED */
;

/* permanent IDs associated to boxes. This way, you can rely on an ID number to identify a BOX function. */
const uint8_t BOXids[] PROGMEM = {
    0, /* ARM */
#if ACC_INSTALLED
    1, /* ANGLE */
    2, /* HORIZON*/
#endif /* ACC_INSTALLED */
#if BARO_INSTALLED && (!defined(SUPPRESS_BARO_ALTHOLD))
    3, /* BARO */
#endif /* BARO_INSTALLED && (!defined(SUPPRESS_BARO_ALTHOLD)) */
#ifdef VARIOMETER
    4, /* VARIO */
#endif /* VARIOMETER */
#if MAG_INSTALLED
    5, /* MAG */
    6, /* HEADFREE */
    7, /* HEADADJ */
#endif /* MAG_INSTALLED */
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
    8, /* CAMSTAB */
#endif /* defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT) */
#if defined(CAMTRIG)
    9, /* CAMTRIG */
#endif /* defined(CAMTRIG) */
#if GPS_INSTALLED
    10, /* GPS HOME */
    11, /* GPS HOLD */
#endif /* GPS_INSTALLED */
#if defined(FIXEDWING) || defined(HELICOPTER)
    12, /* PASSTHRU */
#endif /* defined(FIXEDWING) || defined(HELICOPTER) */
#if defined(BUZZER)
    13, /* BEEPER */
#endif /* defined(BUZZER) */
#if defined(LED_FLASHER)
    14, /* LEDMAX */
    15, /* LEDLOW */
#endif /* defined(LED_FLASHER) */
#if defined(LANDING_LIGHTS_DDR)
    16, /* LLIGHTS */
#endif /* defined(LANDING_LIGHTS_DDR) */
#ifdef INFLIGHT_ACC_CALIBRATION
    17, /* CALIB */
#endif /* INFLIGHT_ACC_CALIBRATION */
#ifdef GOVERNOR_P
    18, /* GOVERNOR */
#endif /* GOVERNOR_P */
#ifdef OSD_SWITCH
    19, /* OSD_SWITCH */
#endif /* OSD_SWITCH */
#if GPS_NAVIGATION_ENABLED
    20, /* MISSION */
    21, /* LAND */
#endif /* GPS_NAVIGATION_ENABLED */
};


/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
static uint8_t prv_gps_modes;       /* Hold the previous box configuration for GPS mode
                                       to determine if the change occured */

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/
#if ACC_INSTALLED
static void FlightController_InitAngleMode(void);
static void FlightController_InitHorizonMode(void);
#endif /* #if ACC_INSTALLED */
static void FlightController_InitBaroMode(void);

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/
#if ACC_INSTALLED
/*******************************************************************************
* Function Name: FlightController_InitAngleMode
********************************************************************************
* Summary:
*  Initializes the ANGLE mode
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void FlightController_InitAngleMode(void)
{
    /* bumpless transfer to Level mode */
    if (!globalFlags.ANGLE_MODE)
    {
        errorAngleI[ROLL] = 0;
        errorAngleI[PITCH] = 0;
        globalFlags.ANGLE_MODE = 1;
    }
}

/*******************************************************************************
* Function Name: FlightController_InitHorizonMode
********************************************************************************
* Summary:
*  Initializes the ANGLE mode
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void FlightController_InitHorizonMode(void)
{
    /* Reset ANGLE mode as HORIZON has higher priority*/
    globalFlags.ANGLE_MODE = 0;

    /* Enable HORIZON Mode */
    if (!globalFlags.HORIZON_MODE)
    {
        errorAngleI[ROLL] = 0;
        errorAngleI[PITCH] = 0;
        globalFlags.HORIZON_MODE = 1;
    }
}
#endif /* #if ACC_INSTALLED */

/*******************************************************************************
* Function Name: FlightController_InitBaroMode
********************************************************************************
* Summary:
*  Initializes the ANGLE mode
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void FlightController_InitBaroMode(void)
{
    if (!globalFlags.BARO_MODE)
    {
        globalFlags.BARO_MODE = 1;
        AltHold = alt.estAltitude;
    #if defined(ALT_HOLD_THROTTLE_MIDPOINT)
        initialThrottleHold = ALT_HOLD_THROTTLE_MIDPOINT;
    #else /* defined(ALT_HOLD_THROTTLE_MIDPOINT) */
        initialThrottleHold = rcCommand[THROTTLE];
    #endif /* defined(ALT_HOLD_THROTTLE_MIDPOINT) */
        errorAltitudeI = 0;
        BaroPID = 0;
    }
}

/*******************************************************************************
* Function Name: FlightController_ManageFlightMode
********************************************************************************
* Summary:
*  This function checks rcOptions and changes the flight mode if required
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void FlightController_ManageFlightMode(void)
{
    /* note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAFE_DELAY is always false */
#if ACC_INSTALLED
    /* Check if ANGLE mode is enabled */
    if ( rcOptions[BOXANGLE] || Failsafe_IsRCFailsafeActive())
    {
        FlightController_InitAngleMode();
    }
    else
    {
        /* Turn off ANGLE Mode flag support */
        globalFlags.ANGLE_MODE = 0;
    }

    /* Check if HORIZON mode is enabled */
    if ( rcOptions[BOXHORIZON] )
    {
        FlightController_InitHorizonMode();
    }
    else
    {
        globalFlags.HORIZON_MODE = 0;
    }
#endif /* ACC_INSTALLED */

    if (rcOptions[BOXARM] == 0)
    {
        globalFlags.OK_TO_ARM = 1;
    }
#if !defined(GPS_LED_INDICATOR)
    if (globalFlags.ANGLE_MODE || globalFlags.HORIZON_MODE)
    {
        STABLEPIN_ON;
    }
    else
    {
        STABLEPIN_OFF;
    }
#endif /* !defined(GPS_LED_INDICATOR) */

#if BARO_INSTALLED
    #if GPS_INSTALLED
        if (GPS_Config.takeover_baro)
        {
            /* Force BARO mode based on Global flag set by GPS if such configuration is set */
            rcOptions[BOXBARO] = (rcOptions[BOXBARO] || globalFlags.GPS_BARO_MODE);
        }
    #endif /* #if GPS_INSTALLED */
    #if (!defined(SUPPRESS_BARO_ALTHOLD))
        /* Enable BARO mode */
        if (rcOptions[BOXBARO])
        {
            FlightController_InitBaroMode();
        }
        else
        {
            globalFlags.BARO_MODE = 0;
        }
    #endif /* #if (!defined(SUPPRESS_BARO_ALTHOLD))*/

#ifdef VARIOMETER
    /* Check if VARIO mode is enabled */
    if (rcOptions[BOXVARIO])
    {
        if (!globalFlags.VARIO_MODE)
        {
            f.VARIO_MODE = 1;
        }
    }
    else
    {
        globalFlags.VARIO_MODE = 0;
    }
#endif /* VARIOMETER */
#endif /* BARO_INSTALLED */

#if MAG_INSTALLED
    /* Check if MAG mode is enbaled */
    if (rcOptions[BOXMAG])
    {
        if (!globalFlags.MAG_MODE)
        {
            globalFlags.MAG_MODE = 1;
            magHold = att.heading;
        }
    }
    else
    {
        globalFlags.MAG_MODE = 0;
    }

    /* Check if HEADFREE mode is enabled */
    if (rcOptions[BOXHEADFREE])
    {
        if (!globalFlags.HEADFREE_MODE)
        {
            globalFlags.HEADFREE_MODE = 1;
        }
    }
    else
    {
        globalFlags.HEADFREE_MODE = 0;
    }

    /* Check if HEADADJ is set */
    if (rcOptions[BOXHEADADJ])
    {
        headFreeModeHold = att.heading; /* acquire new heading */
    }
#endif /* MAG_INSTALLED */

#if GPS_INSTALLED
    uint8_t gps_modes_check = (rcOptions[BOXLAND]<< 3) + (rcOptions[BOXGPSHOME]<< 2) + (rcOptions[BOXGPSHOLD]<<1) + (rcOptions[BOXGPSNAV]);

    if (globalFlags.ARMED )
    {
        /* Check GPS status and armed */
        /* TODO: implement f.GPS_Trusted flag, idea from Dramida - Check for degraded HDOP and sudden speed jumps */
        if (globalFlags.GPS_FIX)
        {
            if (GPS_NumSat > GPS_STABLE_MIN_SATELLITE_NUM)
            {
                /* Check for change since last loop */
                if (prv_gps_modes != gps_modes_check)
                {
                    NAV_error = NAV_ERROR_NONE;
                    if (rcOptions[BOXGPSHOME])
                    {
                        /* RTH has the priotity over everything else */
                        GPS_InitRTH();
                    }
                    else if (rcOptions[BOXGPSHOLD])
                    {
                        /* Position hold has priority over mission execution, but has less priority than RTH*/
                        if (globalFlags.GPS_MODE == GPS_MODE_NAV)
                        {
                            NAV_PauseAtStep = WaypointRecord.number;
                        }

                        globalFlags.GPS_MODE = GPS_MODE_HOLD;

                        /* hold at the current position */
                        GPS_SetNextWP(GPS_CurrentCoord, GPS_CurrentCoord);

                        /* and current altitude */
                        GPS_SetNewAltitude(alt.estAltitude);
                        NAV_state = NAV_STATE_HOLD_INFINIT;
                    }
                    else if (rcOptions[BOXLAND])
                    {
                        /* Land now */
                        globalFlags.GPS_MODE = GPS_MODE_HOLD;
                        /* hold at the current position */
                        GPS_SetNextWP(GPS_CurrentCoord, GPS_CurrentCoord);
                        /* and current altitude */
                        GPS_SetNewAltitude(alt.estAltitude);
                        NAV_state = NAV_STATE_LAND_START;
                    }
                    else if (rcOptions[BOXGPSNAV])
                    {
                        /* Nav mode start */
                        globalFlags.GPS_MODE = GPS_MODE_NAV;
                        globalFlags.GPS_BARO_MODE = true;
                        GPS_PrevWPCoord[LAT] = GPS_CurrentCoord[LAT];
                        GPS_PrevWPCoord[LON] = GPS_CurrentCoord[LON];
                        if (NAV_PauseAtStep != 0)
                        {
                            NAV_NextStep = NAV_PauseAtStep;
                            /* Clear paused step */
                            NAV_PauseAtStep = 0;
                        }
                        else
                        {
                            NAV_NextStep = 1;
                            /* Reset jump counter */
                            NAV_JumpTimes = -10;
                        }
                        NAV_state = NAV_STATE_PROCESS_NEXT;
                    }
                    else    /* None of the GPS Boxes are switched on */
                    {
                        globalFlags.GPS_MODE = GPS_MODE_NONE;
                        globalFlags.GPS_BARO_MODE = false;
                        globalFlags.THROTTLE_IGNORED = false;
                        globalFlags.LAND_IN_PROGRESS = 0;
                        globalFlags.THROTTLE_IGNORED = 0;
                        GPS_ResetNavigation();
                    }
                    /* Save current box modes selection */
                    prv_gps_modes = gps_modes_check;
                }
            } /* GPS_NumSat > GPS_STABLE_MIN_SATELLITE_NUM  */
            else
            {
                /* GPS_NumSat dropped below GPS_STABLE_MIN_SATELLITE_NUM during navigation */
                if (globalFlags.GPS_MODE == GPS_MODE_NAV)
                {
                    NAV_PauseAtStep = WaypointRecord.number;
                    globalFlags.GPS_MODE = GPS_MODE_NONE;
                    GPS_SetNewAltitude(alt.estAltitude);
                    NAV_state = NAV_STATE_NONE;
                    NAV_error = NAV_ERROR_SPOILED_GPS;

                    /* invalidates mode check, to allow re evaluate rcOptions when numsats raised again */
                    prv_gps_modes = 0xff;
                }

                if (globalFlags.GPS_MODE == GPS_MODE_HOLD || globalFlags.GPS_MODE == GPS_MODE_RTH)
                {
                    globalFlags.GPS_MODE = GPS_MODE_NONE;
                    NAV_state = NAV_STATE_NONE;
                    NAV_error = NAV_ERROR_SPOILED_GPS;
                    /* invalidates mode check, to allow re evaluate rcOptions when numsats raised again */
                    prv_gps_modes = 0xff;
                }
                NAV_Bank[0] = 0;
                NAV_Bank[1] = 0;
            }
        } /* globalFlags.GPS_FIX */
        else
        {
            /* GPS Fix dissapeared, very unlikely that we will be able to regain it, abort mission */
            globalFlags.GPS_MODE = GPS_MODE_NONE;
            NAV_state = NAV_STATE_NONE;
            NAV_PauseAtStep = 0;
            NAV_error = NAV_ERROR_GPS_FIX_LOST;
            GPS_ResetNavigation();
            /* Gives a chance to restart mission when regain fix */
            prv_gps_modes = 0xff;
        }
    }  /* copter is armed */
    else
    {
        /* copter is disarmed */
        globalFlags.GPS_MODE = GPS_MODE_NONE;
        globalFlags.GPS_BARO_MODE = false;
        globalFlags.THROTTLE_IGNORED = false;
        NAV_state = NAV_STATE_NONE;
        NAV_PauseAtStep = 0;
        NAV_error = NAV_ERROR_DISARMED;
        GPS_ResetNavigation();
    }
#endif /* GPS_INSTALLED */
}

/*******************************************************************************
* Function Name: FlightController_ManageFlightMode
********************************************************************************
* Summary:
*  This function checks rcOptions and changes the flight mode if required
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
uint32_t FlightController_GetActiveModesFlag(void)
{
    uint32_t activeModeFlag = 0;

#if ACC_INSTALLED
    if (globalFlags.ANGLE_MODE)
    {
        activeModeFlag |= (1 << BOXANGLE);
    }

    if (globalFlags.HORIZON_MODE)
    {
        activeModeFlag |= (1 << BOXHORIZON);
    }
#endif /* ACC_INSTALLED */

#if BARO_INSTALLED && (!defined(SUPPRESS_BARO_ALTHOLD))
    if (globalFlags.BARO_MODE)
    {
        activeModeFlag |= (1 << BOXBARO);
    }
#endif /* BARO_INSTALLED && (!defined(SUPPRESS_BARO_ALTHOLD)) */

#if MAG_INSTALLED
    if (globalFlags.MAG_MODE)
    {
        activeModeFlag |= (1 << BOXMAG);
    }
#if !defined(FIXEDWING)
    if (globalFlags.HEADFREE_MODE)
    {
        activeModeFlag |= (1 << BOXHEADFREE);
    }

    if(rcOptions[BOXHEADADJ])
    {
        activeModeFlag |= (1 << BOXHEADADJ);
    }
#endif /* !defined(FIXEDWING) */
#endif /* MAG_INSTALLED */

#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
    if (rcOptions[BOXCAMSTAB])
    {
        activeModeFlag |= (1 << BOXCAMSTAB);
    }
#endif /* defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT) */

#if defined(CAMTRIG)
    if (rcOptions[BOXCAMTRIG])
    {
        activeModeFlag |= (1 << BOXCAMTRIG);
    }
#endif /* defined(CAMTRIG) */

#if GPS_INSTALLED
    switch (globalFlags.GPS_MODE)
    {
        case GPS_MODE_HOLD:
            activeModeFlag |= 1 << BOXGPSHOLD;
            break;

        case GPS_MODE_RTH:
            activeModeFlag |= 1 << BOXGPSHOME;
            break;

        case GPS_MODE_NAV:
            activeModeFlag |= 1 << BOXGPSNAV;
            break;
    }
#endif /* GPS_INSTALLED */

#if defined(FIXEDWING) || defined(HELICOPTER)
    if (globalFlags.PASSTHRU_MODE)
    {
        activeModeFlag |= (1 << BOXPASSTHRU);
    }
#endif /* defined(FIXEDWING) || defined(HELICOPTER) */

#if defined(BUZZER)
    if (rcOptions[BOXBEEPERON])
    {
        activeModeFlag |= (1 << BOXBEEPERON);
    }
#endif /* defined(BUZZER) */

#if defined(LED_FLASHER)
    if (rcOptions[BOXLEDMAX])
    {
        activeModeFlag |= (1 << BOXLEDMAX);
    }

    if (rcOptions[BOXLEDLOW])
    {
        activeModeFlag |= (1 << BOXLEDLOW);
    }
#endif /* defined(LED_FLASHER) */

#if defined(LANDING_LIGHTS_DDR)
    if (rcOptions[BOXLLIGHTS])
    {
        activeModeFlag |= (1 << BOXLLIGHTS);
    }
#endif /* defined(LANDING_LIGHTS_DDR) */

#if defined(VARIOMETER)
    if (rcOptions[BOXVARIO])
    {
        activeModeFlag |= (1 << BOXVARIO);
    }
#endif /* defined(VARIOMETER) */

#if defined(INFLIGHT_ACC_CALIBRATION)
    if (rcOptions[BOXCALIB])
    {
        activeModeFlag |= (1 << BOXCALIB);
    }
#endif /* defined(INFLIGHT_ACC_CALIBRATION) */

#if defined(GOVERNOR_P)
    if (rcOptions[BOXGOV])
    {
        activeModeFlag |= (1 << BOXGOV);
    }
#endif /* defined(GOVERNOR_P) */

#if defined(OSD_SWITCH)
    if(rcOptions[BOXOSD])
    {
        activeModeFlag |= (1 << BOXOSD);
    }
#endif /* defined(OSD_SWITCH) */

     if (globalFlags.ARMED)
     {
         activeModeFlag |= (1 << BOXARM);
     }

     return (activeModeFlag);
}
